#include "Arm.h"
#include "WPILib.h"
#include <cmath>

Arm::Arm(int vicChannel, int potChannel, int wristChannelIn, int wristChannelOut) :
	motor(vicChannel),
	motorPot(potChannel),
	motorControl(0.1, 0.001, 0.0, &motorPot, &motor, pidPeriod),
	wristSoleIn(wristChannelIn),
	wristSoleOut(wristChannelOut)
{
	motorControl.SetInputRange(limitLow, limitHigh);
	motorControl.SetOutputRange(-1.0, 1.0);
	motorControl.Enable();
	
	motor.SetSafetyEnabled(true);
	motor.SetExpiration(1.0);
	motor.EnableDeadbandElimination(false);
	
	ToggleArm(false);
	
	TogglePID(false);
}

float Arm::Smooth(float cur, float pre, float lim)
{
	if (abs(pre - cur) > lim && cur != 0) {	
		if (pre > cur) return pre - lim;
		else if (pre < cur) return pre + lim;
	}
	return cur;
}

void Arm::Rotate(float input)
{
	if (isPID) {
		float t = motorControl.GetSetpoint() + input * 0.01;
	
		if (t > limitHigh) 		t = limitHigh;
		else if (t < limitLow) 	t = limitLow;
	
		motorControl.SetSetpoint(t);
	}
	else {
		float speed = Smooth(input, motor.Get(), 0.025);
		printf("PID value %d \n", motorPot.GetValue() );
		//if (speed < 0 && speed > -0.1) speed = -0.1;
		
		motor.Set(speed);
	}
}

void Arm::RotateTo(float percent)
{
	if (isPID) {
		float t = limitLow + percent * (limitHigh - limitLow);
		
		motorControl.SetSetpoint(t);
	}
}

void Arm::MoveTo(float height)
{
	if (isPID) {
		height = height < heightLow ? heightLow : height;
		height = height > heightHigh ? heightHigh : height;
	
		RotateTo( (height - heightLow) / (heightHigh - heightLow) );
	}
}

void Arm::TogglePID(bool value)
{
	isPID = value;
	
	if (isPID) {
		motorControl.SetSetpoint(4.5);
		motorControl.Enable();
	}
	else motorControl.Disable();
}

void Arm::TogglePID()
{
	TogglePID(!isPID);
}

void Arm::Deploy()
{
	ToggleArm(true);
}

void Arm::ToggleArm(bool value)
{
	wristSoleOut.Set(value);
	wristSoleIn.Set(!value);
}